import numpy as np
import cv2

cap = cv2.VideoCapture(0)

# params for ShiTomasi corner detection
feature_params = dict( maxCorners = 100,
                       qualityLevel = 0.3,
                       minDistance = 7,
                       blockSize = 7 )

# Parameters for lucas kanade optical flow
lk_params = dict( winSize  = (15,15),
                  maxLevel = 2,
                  criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))

# Create some random colors
color = np.random.randint(0,255,(90500,3))

# Take first frame and find corners in it
keypoints=list()
keypoints1=list()
ret, old_frame = cap.read()
old_frame =cv2.resize(old_frame, (840,680), interpolation=cv2.INTER_AREA)
old_gray = cv2.cvtColor(old_frame, cv2.COLOR_BGR2GRAY)
p0 = cv2.goodFeaturesToTrack(old_gray, mask = None, **feature_params)

new_frame=old_frame.copy()

if p0 is not None and len(p0)>0:
      for x,y in np.float32(p0).reshape(-1,2):
          
          keypoints.append((x,y))
if keypoints is not None and len(keypoints)>0:
    for x,y in keypoints:
        cv2.circle(new_frame, (int(x),y), 3, (255,255,0))
cv2.imshow('new_frame',new_frame)


Hx=None
Hy=None
bitAddPoint=False
def on_EVENT_LBUTTONDOWN(event, x, y, flags, param):
    global p0,bitAddPoint,Hx,Hy
    if event == cv2.EVENT_LBUTTONDOWN:
        xy = "%d,%d" % (x, y)
        Hx=x
        Hy=y
        bitAddPoint=True
        print("点中"+str(x)+","+str(y))
loc = cv2.setMouseCallback("new_frame", on_EVENT_LBUTTONDOWN)
def addPoint(x,y,p0):
    user_points = np.empty([1, 1, 2], dtype=np.float32)
    user_points[0][0] = [x,y]
    if len(user_points) > 0:
       p0 = np.concatenate([p0, user_points])
    return p0;
#for num in range(40,540):  # 迭代 10 到 20 之间的数字
    #p0=addPoint(490,num,p0)
mask = np.zeros_like(old_frame)
bs = cv2.createBackgroundSubtractorKNN(detectShadows=True)  # 背景减除器，设置阴影检测
bs.setHistory(10)

izzzz=0;
while(1):
    ret,frame = cap.read()
    frame =cv2.resize(frame, (840,680), interpolation=cv2.INTER_AREA)
    matFrame=frame.copy()
    fg_mask = bs.apply(frame)   # 获取 foreground mask
    # 对原始帧进行膨胀去噪
    contours, hierarchy = cv2.findContours(fg_mask, cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)  #
    for c in contours:
        # 获取矩形框边界坐标
        x, y, w, h = cv2.boundingRect(c)
        # 计算矩形框的面积
        area = cv2.contourArea(c)
        if 10 < area < 9000:
            izzzz=izzzz+1
            #if(izzzz>500 and izzzz<900):
               #p0=addPoint(x, y,p0)
            cv2.rectangle(matFrame, (x, y), (x + w, y + h), (0, 255, 0), 2)

    if cv2.waitKey(1) & 0xFF == ord('g'):
       print("也是！")
       for num in range(50,680):  # 迭代 10 到 20 之间的数字
           p0=addPoint(num,320,p0)
    frame_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    if(bitAddPoint):
       bitAddPoint=False
       p0=addPoint(Hx,Hy,p0)
       print("出发"+str(Hx))
       
    # calculate optical flow
    p1, st, err = cv2.calcOpticalFlowPyrLK(old_gray, frame_gray, p0, None, **lk_params)
    
    # Select good points
    good_new = p1[st==1]
    good_old = p0[st==1]
   

    # draw the tracks
    for i,(new,old) in enumerate(zip(good_new,good_old)):
        a,b = new.ravel()
        c,d = old.ravel()
        mask = cv2.line(mask, (a,b),(c,d), color[i].tolist(), 1)
        frame = cv2.circle(frame,(a,b),4,color[i].tolist(),-1)
    img = cv2.add(frame,mask)
    cv2.imshow('matFrame',matFrame)
    cv2.imshow('frame',img)
    
  
    # Now update the previous frame and previous points
    old_gray = frame_gray.copy()
    

    p0 = good_new.reshape(-1,1,2)

       
cv2.destroyAllWindows()
cap.release()